#include <cmath>

#include "geo3d.h"

namespace geo3d {

    Point3d interpolate(const Point3d &A, const Point3d &B, double t) {
        return A + (B - A) * t;
    }

    void splitLine(Point3d &a, Point3d &b, int n, std::vector<Point3d> &points) {
        double dx = (b.x - a.x) / n;
        double dy = (b.y - a.y) / n;
        double dz = (b.z - a.z) / n;
        for (int i = 1; i < n; i++) {
            points.push_back({a.x + dx * i, a.y + dy * i, a.z + dz * i});
        }
    }

    void findClosestPoint(const std::vector<Point3d> &surface_points, const Point3d &p, INDEX &index, double &dist) {
        dist = 1e9;
        index = -1;
        for (INDEX i = 0; i < surface_points.size(); i++) {
            double tmp_dist = (surface_points[i].x - p.x) * (surface_points[i].x - p.x) + (surface_points[i].y - p.y) * (surface_points[i].y - p.y) + (surface_points[i].z - p.z) * (surface_points[i].z - p.z);
            if (tmp_dist < dist) {
                dist = tmp_dist;
                index = i;
            }
        }
    }

    void findClosestLine(const std::vector<Point3d> &bottom_points, const std::vector<Point3d> &top_points, const Point3d &bottom_p, const Point3d &top_p, INDEX &index, double &dist) {
        dist = 1e9;
        index = -1;
        INDEX point_count = bottom_points.size();

        for (INDEX i = 0; i < point_count; i++) {
            double bottom_dist = std::sqrt((bottom_points[i].x - bottom_p.x) * (bottom_points[i].x - bottom_p.x) + (bottom_points[i].y - bottom_p.y) * (bottom_points[i].y - bottom_p.y) + (bottom_points[i].z - bottom_p.z) * (bottom_points[i].z - bottom_p.z));
            double top_dist = std::sqrt((top_points[i].x - top_p.x) * (top_points[i].x - top_p.x) + (top_points[i].y - top_p.y) * (top_points[i].y - top_p.y) + (top_points[i].z - top_p.z) * (top_points[i].z - top_p.z));
            if (bottom_dist + top_dist < dist) {
                dist = bottom_dist + top_dist;
                index = i;
            }
        }
    }

    // 利用指数方法在四面体内采样一个点
    // 参数：四面体的四个顶点 A, B, C, D 以及随机数生成器 gen
    Point3d samplePointInTetrahedron(const Point3d &A, const Point3d &B, const Point3d &C, const Point3d &D, std::mt19937 &gen) {
        // 定义均匀分布 [0,1)
        std::uniform_real_distribution<double> uniform(0.0, 1.0);
        
        // 产生 4 个均匀随机数
        double u1 = uniform(gen);
        double u2 = uniform(gen);
        double u3 = uniform(gen);
        double u4 = uniform(gen);
        
        // 取负对数，得到服从指数分布的随机变量
        double x1 = -std::log(u1);
        double x2 = -std::log(u2);
        double x3 = -std::log(u3);
        double x4 = -std::log(u4);
        
        // 求和归一化
        double sum = x1 + x2 + x3 + x4;
        double t1 = x1 / sum;
        double t2 = x2 / sum;
        double t3 = x3 / sum;
        double t4 = x4 / sum;
        
        // 计算采样点：P = t1*A + t2*B + t3*C + t4*D
        Point3d P = A * t1 + B * t2 + C * t3 + D * t4;
        return P;
    }

    double distance(Point3d a, Point3d b) {
        return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z));
    }
    double distance2(Point3d a, Point3d b) {
        return (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z);
    }
    double magnitude(Point3d p) {
        return std::sqrt(p.x * p.x + p.y * p.y + p.z * p.z);
    }
    double magnitude2(Point3d p) {
        return p.x * p.x + p.y * p.y + p.z * p.z;
    }
    Point3d crossProduct(Point3d a, Point3d b) {
        return Point3d(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
    }
    double dotProduct(Point3d a, Point3d b) {
        return a.x * b.x + a.y * b.y + a.z * b.z;
    }
    double cosAngle(Point3d a, Point3d b, Point3d c) {
        Point3d ab = Point3d(b.x - a.x, b.y - a.y, b.z - a.z);
        Point3d ac = Point3d(c.x - a.x, c.y - a.y, c.z - a.z);
        return dotProduct(ab, ac) / (magnitude(ab) * magnitude(ac));
    }
    double triangleArea(Point3d a, Point3d b, Point3d c) {
        Point3d ab = Point3d(b.x - a.x, b.y - a.y, b.z - a.z);
        Point3d ac = Point3d(c.x - a.x, c.y - a.y, c.z - a.z);
        Point3d cross = crossProduct(ab, ac);
        return 0.5 * magnitude(cross);
    }
    // Compute the incenter of the tetrahedron
    Point3d computeIncenter(Point3d a, Point3d b, Point3d c, Point3d d) {
        double A1 = triangleArea(b, c, d); // Opposite a
        double A2 = triangleArea(a, c, d); // Opposite b
        double A3 = triangleArea(c, b, d); // Opposite c
        double A4 = triangleArea(a, b, c); // Opposite d

        double sumA = A1 + A2 + A3 + A4;

        // Weighted sum of vertex coordinates
        return Point3d(
            (A1 * a.x + A2 * b.x + A3 * c.x + A4 * d.x) / sumA,
            (A1 * a.y + A2 * b.y + A3 * c.y + A4 * d.y) / sumA,
            (A1 * a.z + A2 * b.z + A3 * c.z + A4 * d.z) / sumA);
    }
    // Compute the mass center of the tetrahedron
    Point3d computeMasscenter(Point3d a, Point3d b, Point3d c, Point3d d) {
        return Point3d((a.x + b.x + c.x + d.x) / 4, (a.y + b.y + c.y + d.y) / 4, (a.z + b.z + c.z + d.z) / 4);
    }

    bool lineBoxIntersection(const Line& line, const Point3d& min_bnd, const Point3d& max_bnd) {
        double t_min = -1e9;
        double t_max = 1e9;

        // 定义 lambda 函数来更新 t_min 和 t_max
        auto update = [&](double p, double q_min, double q_max) -> bool {
            if (p == 0) {
                return q_min * q_max <= 0;
            }
            q_min /= p;
            q_max /= p;
            if(q_min > q_max) {
                std::swap(q_min, q_max);
            }
            t_min = std::max(t_min, q_min);
            t_max = std::min(t_max, q_max);
            return t_min <= t_max;
        };

        // 检查 x 轴方向
        if (!update(line.a.x - line.b.x, line.a.x - min_bnd.x, line.a.x - max_bnd.x)) {
            return false;
        }
        // 检查 y 轴方向
        if (!update(line.a.y - line.b.y, line.a.y - min_bnd.y, line.a.y - max_bnd.y)) {
            return false;
        }
        // 检查 z 轴方向
        if (!update(line.a.z - line.b.z, line.a.z - min_bnd.z, line.a.z - max_bnd.z)) {
            return false;
        }
        return true;
    }

    int lineTriangleIntersection(const Line& line, const Point3d& a, const Point3d& b, const Point3d& c, Point3d& P) {
        Point3d ab = Point3d(b.x - a.x, b.y - a.y, b.z - a.z);
        Point3d ac = Point3d(c.x - a.x, c.y - a.y, c.z - a.z);
        Point3d n = crossProduct(ab, ac);
        double dot = dotProduct(n, Point3d(line.b.x - line.a.x, line.b.y - line.a.y, line.b.z - line.a.z));
        // 直线与三角形所在平面平行
        if (fabs(dot) < 1e-6) {
            return -1;
        }
        double t = dotProduct(n, Point3d(a.x - line.a.x, a.y - line.a.y, a.z - line.a.z)) / dot;
        P = Point3d(line.a.x + t * (line.b.x - line.a.x), line.a.y + t * (line.b.y - line.a.y), line.a.z + t * (line.b.z - line.a.z));
        Point3d ap = Point3d(P.x - a.x, P.y - a.y, P.z - a.z);
        Point3d bp = Point3d(P.x - b.x, P.y - b.y, P.z - b.z);
        Point3d cp = Point3d(P.x - c.x, P.y - c.y, P.z - c.z);
        double area = magnitude(crossProduct(ab, ac));
        double area1 = magnitude(crossProduct(ap, bp));
        double area2 = magnitude(crossProduct(bp, cp));
        double area3 = magnitude(crossProduct(cp, ap));
        return fabs(area - (area1 + area2 + area3)) < 1e-6;
    }

}; // namespace geo3d
